/**
 * @file NewProtocolCurtisSeer.c
 * @author SoBigDog (623644973lubin@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-11-16
 * 
 * @copyright Copyright (c) 2020
 * 
 */
#include "NewProtocolCurtisSeer.h"

#define FORK_UP_Vel			0x20
#define FORK_DOWN_Vel		0x30 

static int16_t _speedVal = 0;
static int16_t _forkVel = 0;
static int16_t _lower_flag = 0;
static int16_t _lift_flag = 0;

static bool encode(enum motorDriverCmdType ct, int32_t val)
{
	CAN_TxHeaderTypeDef head;
	uint32_t TxMailbox;
	uint8_t buff[8];
	
	if (DRV_CMD_TARGET_SPEED == ct) {
		/* 上层传入RPM */
		_speedVal = val;
		return false;
	} else if (DRV_CMD_TARGET_POS == ct) {
		uint8_t driveRawDir;
		uint16_t driveRawSpeed;
		int16_t steerRawAngle;
		/*******************************************************************************************************************
		 * add control fock motor lift and lower /J3 is 1 /J3、J4 is 1
		 *******************************************************************************************************************/
		 if(_lift_flag == 1){
			driveRawDir = (_speedVal < 0) ? (0x05 | FORK_UP_Vel): (0x03 | FORK_UP_Vel);  //J4 set1  ??
		}else if(_lower_flag == 1){
			driveRawDir = (_speedVal < 0) ? (0x05 | FORK_DOWN_Vel): (0x03 | FORK_DOWN_Vel);  //J3/J4 set1  ??
		}else{
			driveRawDir = (_speedVal < 0) ? 0x05: 0x03;
		}

		driveRawSpeed = (_speedVal < 0) ? -_speedVal : _speedVal;

		steerRawAngle = val;
		
		if (steerRawAngle > 1200)
			steerRawAngle = 1200;
		else if (steerRawAngle < -1200)
			steerRawAngle = -1200;
	
		head.StdId = 0x203;
		head.IDE = CAN_ID_STD;
		head.RTR = CAN_RTR_DATA;
		head.DLC = 8;
		head.TransmitGlobalTime = DISABLE;

		buff[0] = driveRawDir;
		buff[1] = driveRawSpeed & 0xFF;
		buff[2] = driveRawSpeed >> 8;
		buff[3] = 0x0a;			//acc
		buff[4] = 0x0a;			//acc
		buff[5] = (uint8_t)((steerRawAngle) & 0xFF);	//theta L
		buff[6] = (steerRawAngle) >> 8;					//theta H
		buff[7] = 0x50;
		HAL_CAN_AddTxMessage(&hcan, &head, buff, &TxMailbox);

		head.StdId = 0x303;
		buff[0] = ((uint8_t)_lift_flag << 1) | ((uint8_t)_lower_flag << 2);
		if (_lift_flag == 1)
		{
			buff[1] = _forkVel & 0xFF;
			buff[2] = _forkVel >> 8;		
		} else {
			buff[1] = 0x00;
			buff[2] = 0x00;
		}

		if (_lower_flag == 1) {
			buff[3] = (uint16_t)(_forkVel * 16.383f) & 0xff;
			buff[4] = (uint16_t)(_forkVel * 16.383f) >> 8;
		} else {
			buff[3] = 0x00;
			buff[4] = 0x00;
		}
		buff[5] = 0x00;							//theta L
		buff[6] = 0x00;							//theta H
		buff[7] = 0x00;

		HAL_CAN_AddTxMessage(&hcan, &head, buff, &TxMailbox);

	} else if (DRV_CMD_FORK_VEL == ct) {
		_forkVel = val;
		if (_forkVel > 0) {
			_lift_flag = true;
			_lower_flag = false;
		} else if (_forkVel < 0) {
			_lift_flag = false;
			_lower_flag = true;
			_forkVel = -_forkVel;
		} else if (_forkVel == 0) {
			_lift_flag = false;
			_lower_flag = false;
		}

		return false;
	}
	return true;
}


static int decode(enum motorDriverCmdType **ct, int32_t **val)
{
	//(*ct)[0]
	return 0;
}

const struct driver_protocol ProtocolCurtisSeer = {
	.encode = encode,
	.decode = decode,
};
